// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Program.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Benchmark
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.IO;
    using System.Linq;
    using System.Net.Sockets;
    using System.Runtime.InteropServices;
    using System.Threading;
    using System.Windows.Media; 
    using CmdLine;

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras.Webcam;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    
    /// <summary>
    /// This program performs two basic functions:
    /// 1) In mapping mode it will record the current ground truth using the up-facing camera when signaled via tcp/ip or spacebar.  
    ///    The output is an XML file with the list of ground truth readings in chronological order.
    /// 2) In navigation mode it will compare the current ground truth to the ground truth captured during mapping when signaled via tcp/ip.
    ///    The output is a text file containing the results of all the comparisons.
    /// </summary>
    public class Program
    {
        /// <summary>
        /// Main program for the Navigation Benchmark System.
        /// </summary>
        /// <param name="args">Command line arguments</param>
        public static void Main(string[] args)
        {
            NavBenchmarkArguments arguments;

            // parse the command line
            try
            {
                arguments = CommandLine.Parse<NavBenchmarkArguments>();
            }
            catch (CommandLineException exception)
            {
                Console.WriteLine(exception.ArgumentHelp.Message);
                Console.WriteLine(exception.ArgumentHelp.GetHelpText(Console.BufferWidth));
                return;
            }

            NavBenchmark navBenchmark = new NavBenchmark(
                arguments.Mode, 
                int.Parse(arguments.Port), 
                arguments.InputFolder, 
                arguments.OutputFolder, 
                arguments.CameraFocus == "null" ? null : (int?)int.Parse(arguments.CameraFocus),
                arguments.CameraExposure == "null" ? null : (int?)int.Parse(arguments.CameraExposure),
                arguments.Camera,
                arguments.DistanceErrorThreshold == "null" ? null : (int?)int.Parse(arguments.DistanceErrorThreshold));

            if (!navBenchmark.Initialize())
            {
                Console.WriteLine("Unable to initialize navigation benchmark.  Exiting.");
                return;
            }

            ConsoleKey key;

            while (true)
            {
                key = Console.ReadKey().Key;

                if (arguments.Mode == "calib")
                {
                    if (key == ConsoleKey.Escape)
                    {
                        break;
                    }
                }

                if (arguments.Mode == "map")
                {
                    if (key == ConsoleKey.Spacebar)
                    {
                        byte[] bytes = { 0 };
                        navBenchmark.OnSignaled(bytes);
                    }

                    if (key == ConsoleKey.Escape)
                    {
                        break;
                    }
                }

                if (arguments.Mode == "nav")
                {
                    if (key == ConsoleKey.Escape)
                    {
                        Console.WriteLine(string.Empty);
                        Console.WriteLine("****************************************");
                        Console.WriteLine("You pressed <esc> to exit the run.");
                        Console.WriteLine("Press <esc> again to exit.");
                        Console.WriteLine("Press any other key to continue the run normally.");
                        Console.WriteLine("****************************************");
                        Console.WriteLine(string.Empty);

                        ConsoleKey verificationKey = Console.ReadKey().Key;
                        if (verificationKey == ConsoleKey.Escape)
                        {
                            break;
                        }
                        else
                        {
                            Console.WriteLine("<esc> cancelled. Waiting for signal.");
                        }
                    }

                    // The following are primarily for testing/debugging.
                    // They are not documented.
                    int num;
                    if (int.TryParse(((char)key).ToString(), out num))
                    {
                        // send a fake nav message
                        NavBenchmark.Message message = new NavBenchmark.Message() { LandmarkId = (byte)num, RunId = 999 };
                        navBenchmark.OnSignaled(TcpCom.ConvertToByteArray(message, Marshal.SizeOf(message)));
                    }
                }
            }

            navBenchmark.Shutdown();

            Console.WriteLine("\nGoodbye.");
        }
    }
}
